#!/usr/bin/env python
import socket
import itertools
import sys

from dm_reg import *
from dm_utils import *
from openocd import *

def check_busy(ocd):
    sbcs.bits = int(ocd.dmi_read(sbcs.addr), 16)
    return sbcs.sbbusy == 1

if __name__ == "__main__":
    addr = int(sys.argv[1], 16)
    data = int(sys.argv[2], 16)

    with OpenOcd(verbose=False) as ocd:
        while check_busy(ocd):
            print("waiting")
            pass

        sbaddress0.bits = addr & 0xffffffff
        sbaddress1.bits = addr >> 32
        sbdata0.bits = data

        ocd.dmi_write(sbaddress1.addr, sbaddress1.bits)
        ocd.dmi_write(sbaddress0.addr, sbaddress0.bits)
        ocd.dmi_write(sbdata0.addr, sbdata0.bits)
